Added interpolate filter
authorparkrrrr <parkrrrr@f51c46e8-681c-474f-0cfe-069cfd0219fb>
Tue, 28 Mar 2006 22:13:53 +0000 (22:13 +0000)
committerparkrrrr <parkrrrr@f51c46e8-681c-474f-0cfe-069cfd0219fb>
Tue, 28 Mar 2006 22:13:53 +0000 (22:13 +0000)
gpsbabel/Makefile
gpsbabel/filter_vecs.c
gpsbabel/grtcirc.c
gpsbabel/grtcirc.h
gpsbabel/interpolate.c [new file with mode: 0644]
gpsbabel/readme.xml
gpsbabel/route.c
gpsbabel/testo

index 705ad3cf39783080d647fe16b1a0e0d8031ee7c0..a9677d2f37829d5dd7816b7a3b60864632be810f 100644 (file)
@@ -51,7 +51,7 @@ FMTS=magproto.o gpx.o geo.o mapsend.o mapsource.o garmin_tables.o \
 
 FILTERS=position.o duplicate.o arcdist.o polygon.o smplrout.o \
        reverse_route.o sort.o stackfilter.o trackfilter.o discard.o \
-       nukedata.o
+       nukedata.o interpolate.o
 
 OSJEEPS=jeeps/gpslibusb.o
 JEEPS=jeeps/gpsapp.o jeeps/gpscom.o \
@@ -182,7 +182,7 @@ msvc-build:
        echo $(OBJS) > objs.lst
        LINK.EXE /NOLOGO @objs.lst ./msvc/expat/libexpat.lib /out:gpsbabel.exe 
 
-# Machine generated from here down.  
+# Machine generated from here down. 
 an1.o: an1.c defs.h queue.h gbtypes.h cet.h cet_util.h an1sym.h
 arcdist.o: arcdist.c defs.h queue.h gbtypes.h cet.h cet_util.h \
   filterdefs.h grtcirc.h
@@ -195,8 +195,6 @@ brauniger_iq.o: brauniger_iq.c defs.h queue.h gbtypes.h cet.h cet_util.h \
   jeeps/gpsmath.h jeeps/gpsmem.h jeeps/gpsrqst.h jeeps/gpsinput.h \
   jeeps/gpsproj.h
 cet.o: cet.c defs.h queue.h gbtypes.h cet.h cet_util.h
-cetus.o: cetus.c defs.h queue.h gbtypes.h cet.h cet_util.h \
-  coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h
 cet_util.o: cet_util.c defs.h queue.h gbtypes.h cet.h cet_util.h \
   cet/iso_8859_1.h cet/iso_8859_15.h cet/ansi_x3_4_1968.h cet/cp1252.h \
   cet/atarist.h cet/baltic.h cet/bs_4730.h cet/bs_viewdata.h cet/cp1250.h \
@@ -224,6 +222,8 @@ cet_util.o: cet_util.c defs.h queue.h gbtypes.h cet.h cet_util.h \
   cet/nextstep.h cet/nf_z_62_010.h cet/nf_z_62_010__1973_.h \
   cet/ns_4551_1.h cet/ns_4551_2.h cet/pt.h cet/pt2.h cet/sami.h \
   cet/sen_850200_b.h cet/sen_850200_c.h cet/tcvn.h cet/viscii.h cet/vps.h
+cetus.o: cetus.c defs.h queue.h gbtypes.h cet.h cet_util.h \
+  coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h
 coastexp.o: coastexp.c defs.h queue.h gbtypes.h cet.h cet_util.h \
   xmlgeneric.h uuid.h
 compegps.o: compegps.c defs.h queue.h gbtypes.h cet.h cet_util.h \
@@ -299,18 +299,20 @@ ignrando.o: ignrando.c defs.h queue.h gbtypes.h cet.h cet_util.h \
 inifile.o: inifile.c defs.h queue.h gbtypes.h cet.h cet_util.h inifile.h
 internal_styles.o: internal_styles.c defs.h queue.h gbtypes.h cet.h \
   cet_util.h
+interpolate.o: interpolate.c defs.h queue.h gbtypes.h cet.h cet_util.h \
+  filterdefs.h grtcirc.h
 kml.o: kml.c defs.h queue.h gbtypes.h cet.h cet_util.h xmlgeneric.h
 lowranceusr.o: lowranceusr.c defs.h queue.h gbtypes.h cet.h cet_util.h
-maggeo.o: maggeo.c defs.h queue.h gbtypes.h cet.h cet_util.h xmlgeneric.h \
-  magellan.h
-magnav.o: magnav.c defs.h queue.h gbtypes.h cet.h cet_util.h \
-  coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h
 mag_pdb.o: mag_pdb.c defs.h queue.h gbtypes.h cet.h cet_util.h \
   coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h jeeps/gpsmath.h \
   jeeps/gps.h jeeps/../defs.h jeeps/gpsport.h jeeps/gpsserial.h \
   jeeps/gpssend.h jeeps/gpsread.h jeeps/gpsutil.h jeeps/gpsapp.h \
   jeeps/gpsprot.h jeeps/gpscom.h jeeps/gpsfmt.h jeeps/gpsmath.h \
   jeeps/gpsmem.h jeeps/gpsrqst.h jeeps/gpsinput.h jeeps/gpsproj.h
+maggeo.o: maggeo.c defs.h queue.h gbtypes.h cet.h cet_util.h xmlgeneric.h \
+  magellan.h
+magnav.o: magnav.c defs.h queue.h gbtypes.h cet.h cet_util.h \
+  coldsync/palm.h coldsync/../gbtypes.h coldsync/pdb.h
 magproto.o: magproto.c defs.h queue.h gbtypes.h cet.h cet_util.h \
   magellan.h
 main.o: main.c defs.h queue.h gbtypes.h cet.h cet_util.h filterdefs.h
@@ -390,8 +392,8 @@ tpo.o: tpo.c defs.h queue.h gbtypes.h cet.h cet_util.h jeeps/gpsmath.h \
 trackfilter.o: trackfilter.c defs.h queue.h gbtypes.h cet.h cet_util.h \
   filterdefs.h strptime.h
 unicsv.o: unicsv.c defs.h queue.h gbtypes.h cet.h cet_util.h csv_util.h
-util_crc.o: util_crc.c
 util.o: util.c defs.h queue.h gbtypes.h cet.h cet_util.h
+util_crc.o: util_crc.c
 uuid.o: uuid.c uuid.h
 vcf.o: vcf.c defs.h queue.h gbtypes.h cet.h cet_util.h jeeps/gpsmath.h \
   jeeps/gps.h jeeps/../defs.h jeeps/gpsport.h jeeps/gpsserial.h \
@@ -494,5 +496,5 @@ jeeps/gpsutil.o: jeeps/gpsutil.c jeeps/gps.h jeeps/../defs.h \
   jeeps/gpsinput.h jeeps/gpsproj.h
 shapelib/dbfopen.o: shapelib/dbfopen.c shapelib/shapefil.h
 shapelib/shpopen.o: shapelib/shpopen.c shapelib/shapefil.h
-internal_styles.c: mkstyle.sh style/arc.style style/cambridge.style style/csv.style style/cup.style style/custom.style style/dna.style style/fugawi.style style/garmin301.style style/garmin_poi.style style/geonet.style style/gpsdrive.style style/gpsdrivetrack.style style/gpsman.style style/mapconverter.style style/mxf.style style/nima.style style/openoffice.style style/README.style style/s_and_t.style style/saplus.style style/tabsep.style style/xmap.style style/xmapwpt.style
+internal_styles.c: mkstyle.sh style/README.style style/arc.style style/cambridge.style style/csv.style style/cup.style style/custom.style style/dna.style style/fugawi.style style/garmin301.style style/garmin_poi.style style/geonet.style style/gpsdrive.style style/gpsdrivetrack.style style/gpsman.style style/mapconverter.style style/mxf.style style/nima.style style/openoffice.style style/s_and_t.style style/saplus.style style/tabsep.style style/xmap.style style/xmapwpt.style
        ./mkstyle.sh > internal_styles.c || (rm -f internal_styles.c ; exit 1)
index 3b4a5feb86883435e6700312c4e5da1f88e0ee4e..b724da9e4122dd317e9070e28301b1ae6455d41c 100644 (file)
@@ -40,6 +40,7 @@ extern filter_vecs_t stackfilt_vecs;
 extern filter_vecs_t trackfilter_vecs;
 extern filter_vecs_t discard_vecs;
 extern filter_vecs_t nuke_vecs;
+extern filter_vecs_t interpolatefilt_vecs;
 
 static
 fl_vecs_t filter_vec_list[] = {
@@ -103,6 +104,11 @@ fl_vecs_t filter_vec_list[] = {
                "nuketypes",
                "Remove all waypoints, tracks, or routes"
        },
+       {
+               &interpolatefilt_vecs,
+               "interpolate",
+               "Interpolate between trackpoints"
+       },
         {
                NULL,
                NULL,
index d5e5cddbb98822804ed6d8de743841906cea3569..3676eeaa5a2fe4d8f0b70ca39cb1404c127f8a85 100644 (file)
@@ -203,3 +203,83 @@ double linedist(double lat1, double lon1,
   return 0;
 }
 
+/* 
+ * Compute the position of a point partially along the geodesic from 
+ * lat1,lon1 to lat2,lon2
+ * 
+ * Ref: http://mathworld.wolfram.com/RotationFormula.html
+ */
+
+void linepart(double lat1, double lon1,
+               double lat2, double lon2,
+               double frac,
+               double *reslat, double *reslon ) {
+
+  double x1,y1,z1;
+  double x2,y2,z2;
+  double xa,ya,za,la;
+  double xr, yr, zr;
+  double xx, yx, zx;
+  
+  double theta = 0;
+  double phi = 0;
+  double cosphi = 0;
+  double sinphi = 0;
+  
+  /* degrees to radians */
+  lat1 *= M_PI/180.0;  lon1 *= M_PI/180.0;
+  lat2 *= M_PI/180.0;  lon2 *= M_PI/180.0;
+
+  /* polar to ECEF rectangular */
+  x1 = cos(lon1)*cos(lat1); y1 = sin(lat1); z1 = sin(lon1)*cos(lat1);
+  x2 = cos(lon2)*cos(lat2); y2 = sin(lat2); z2 = sin(lon2)*cos(lat2);
+
+  /* 'a' is the axis; the line that passes through the center of the earth
+   * and is perpendicular to the great circle through point 1 and point 2 
+   * It is computed by taking the cross product of the '1' and '2' vectors.*/
+  crossproduct( x1, y1, z1, x2, y2, z2, &xa, &ya, &za );
+  la = sqrt(xa*xa+ya*ya+za*za);
+
+  if ( la ) {
+    xa /= la;
+    ya /= la;
+    za /= la;
+  }
+  *reslat = lat1;
+  *reslon = lon1;
+  /* if la is zero, the points are either equal or directly opposite 
+   * each other.  Either way, there's no single geodesic, so we punt. */
+  if ( la ) {
+    crossproduct( x1, y1, z1, xa, ya, za, &xx, &yx, &zx );
+   
+    
+    theta = atan2( dotproduct(xx,yx,zx,x2,y2,z2),
+                  dotproduct(x1,y1,z1,x2,y2,z2));
+    
+    phi = frac * theta;
+    cosphi = cos(phi);
+    sinphi = sin(phi);
+    
+    
+    /* The second term of the formula from the mathworld reference is always
+     * zero, because r (lat1,lon1) is always perpendicular to n (a here) */
+    xr = x1*cosphi + xx * sinphi;
+    yr = y1*cosphi + yx * sinphi;
+    zr = z1*cosphi + zx * sinphi;
+    
+    if ( xr > 1 ) xr = 1;
+    if ( xr < -1 ) xr = -1;
+    if ( yr > 1 ) yr = 1;
+    if ( yr < -1 ) yr = -1;
+    if ( zr > 1 ) zr = 1;
+    if ( zr < -1 ) zr = -1;
+    
+    *reslat = asin(yr) * 180 / M_PI;   
+    if( xr == 0 && zr == 0 ) {
+      *reslon = 0;
+    }
+    else {
+      *reslon = atan2( zr, xr ) * 180 / M_PI;
+    } 
+  }
+}
index de84aea4c60983ea37f4a0f1a32b66b8b5a18601..250e0d2ab4941960f8edbcc8f466df0f2e784bd0 100644 (file)
@@ -25,3 +25,9 @@ double linedist(double lat1, double lon1,
                double lat3, double lon3 );
 
 double tomiles( double rads );
+
+void linepart(double lat1, double lon1,
+               double lat2, double lon2,
+               double frac,
+               double *reslat, double *reslon ); 
+       
diff --git a/gpsbabel/interpolate.c b/gpsbabel/interpolate.c
new file mode 100644 (file)
index 0000000..f9cbdb2
--- /dev/null
@@ -0,0 +1,164 @@
+/*
+    Interpolate filter
+
+    Copyright (C) 2002 Robert Lipe, robertlipe@usa.net
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
+
+ */
+
+#include "defs.h"
+#include "filterdefs.h"
+#include "grtcirc.h"
+
+#define MYNAME "Interpolate filter"
+
+static char *opt_interval = NULL;
+int interval = 0;
+static char *opt_dist = NULL;
+double dist = 0;
+
+static
+arglist_t interpfilt_args[] = {
+       {"time", &opt_interval, "Time interval in seconds", NULL, 
+               ARGTYPE_BEGIN_EXCL | ARGTYPE_BEGIN_REQ | ARGTYPE_INT, 
+               "0", NULL },
+       {"distance", &opt_dist, "Distance interval in miles or kilometers", 
+               NULL, ARGTYPE_END_EXCL | ARGTYPE_END_REQ | ARGTYPE_STRING, 
+               ARG_NOMINMAX },
+       ARG_TERMINATOR
+};
+
+void 
+interpfilt_process(void)
+{
+       queue *backuproute = NULL;
+       queue *elem, *tmp, *elem2, *tmp2;
+       route_head *rte_new;
+       int count = 0;
+       int first = 0;
+       double lat1, lon1;
+       int time1;
+       int timen;
+       double distn;
+       double curdist;
+       double rt1, rn1, rt2, rn2;
+       
+       track_backup( &count, &backuproute );
+        route_flush_all_tracks();
+        QUEUE_FOR_EACH( backuproute, elem, tmp )
+       {
+               route_head *rte_old = (route_head *)elem;
+               
+               rte_new = route_head_alloc();
+               rte_new->rte_name = xstrdup( rte_old->rte_name );
+               rte_new->rte_desc = xstrdup( rte_old->rte_desc );
+               rte_new->fs = fs_chain_copy( rte_old->fs );
+               rte_new->rte_num = rte_old->rte_num;
+               track_add_head( rte_new );
+               first = 1;
+               QUEUE_FOR_EACH( &rte_old->waypoint_list, elem2, tmp2 ) 
+               {
+                   waypoint *wpt = (waypoint *)elem2;
+                   if ( first ) {
+                       first = 0;
+                   }
+                   else {
+                       if ( opt_interval && 
+                               wpt->creation_time - time1 > interval ) {
+                           for ( timen = time1+interval; 
+                                       timen < wpt->creation_time;
+                                       timen += interval ) {
+                               waypoint *wpt_new = waypt_dupe(wpt);
+                               wpt_new->creation_time = timen;
+                               xfree(wpt_new->shortname);
+                               xfree(wpt_new->description);
+                               wpt_new->shortname = wpt_new->description = 0;
+                               linepart( lat1, lon1, 
+                                         wpt->latitude, wpt->longitude,
+                                         (double)(timen-time1)/
+                                         (double)(wpt->creation_time-time1),
+                                         &wpt_new->latitude,
+                                         &wpt_new->longitude );
+                               track_add_wpt( rte_new, wpt_new);
+                           }
+                       }
+                       else if ( opt_dist ) {
+                           rt1 = lat1 * M_PI / 180;
+                           rn1 = lon1 * M_PI / 180;
+                           rt2 = wpt->latitude * M_PI / 180;
+                           rn2 = wpt->longitude * M_PI / 180;
+                           curdist = gcdist( rt1, rn1, rt2, rn2 );
+                           curdist = tomiles(curdist);
+                           if ( curdist > dist ) {     
+                             for ( distn = dist; 
+                                       distn < curdist;
+                                       distn += dist ) {
+                               waypoint *wpt_new = waypt_dupe(wpt);
+                               wpt_new->creation_time = distn/curdist*
+                                       (wpt->creation_time - time1) + time1;
+                               xfree(wpt_new->shortname);
+                               xfree(wpt_new->description);
+                               wpt_new->shortname = wpt_new->description = 0;
+                               linepart( lat1, lon1, 
+                                         wpt->latitude, wpt->longitude,
+                                         distn/curdist,
+                                         &wpt_new->latitude,
+                                         &wpt_new->longitude );
+                               track_add_wpt( rte_new, wpt_new);
+                             }
+                           }
+                       }
+                   }
+                   track_add_wpt( rte_new, waypt_dupe(wpt));
+                   
+                   lat1 = wpt->latitude;
+                   lon1 = wpt->longitude;
+                   time1 = wpt->creation_time;
+               }
+       }
+       route_flush( backuproute );
+       xfree( backuproute );
+}
+
+void
+interpfilt_init(const char *args) {
+       
+        char *fm;
+       if ( opt_interval && opt_dist ) {
+               fatal( MYNAME ": Can't interpolate on both time and distance.\n");
+       }
+       else if ( opt_interval ) {
+               interval = atoi(opt_interval);
+       }
+       else if ( opt_dist ) {
+                dist = strtod(opt_dist, &fm);
+               if ((*fm == 'k') || (*fm == 'K')) {
+                    /* distance is kilometers, convert to miles */
+                    dist *= .6214;
+               }
+       }
+       else {
+               fatal( MYNAME ": No interval specified.\n");
+       }
+}
+
+filter_vecs_t interpolatefilt_vecs = {
+       interpfilt_init,
+       interpfilt_process,
+       NULL,
+       NULL,
+       interpfilt_args
+};
index 4cbb969cd85add205b29e7fd72300b5973e19360..5c85ac399ce770c85a7f8e787d547b31c5393d0b 100644 (file)
@@ -2159,5 +2159,26 @@ gpsbabel -t \
         <screen format="linespecific">gpsbabel -i gpx -f bigfile.gpx -x nuketypes,waypoints,routes -o gpx -F tracksonly.gpx
        </screen>
       </section>
+      <section>
+          <title>INTERPOLATE</title>
+       <para>
+       This filter modifies any tracks so that either the distance or the time
+       between consecutive points is no less than the specified interval. Where
+       points are missing, the filter fills them in by following a straight 
+       line (actually a great circle) between the adjacent points.  You
+       must specify either the "distance" or the "time" option.
+       </para>
+       <screen format="linespecific">gpsbabel -i gpx -f track.gpx -x interpolate,time=10 -o gpx -f newtrack.gpx</screen>
+       <para>Reads track.gpx and inserts points wherever two adjacent
+       trackpoints are more than 10 seconds apart</para>
+       <screen format="linespecific">gpsbabel -i gpx -f track.gpx -x interpolate,distance=15k -o gpx -f newtrack.gpx</screen>
+       <para>Reads track.gpx and inserts points wherever two adjacent
+       trackpoints are more than 15 kilometers apart</para>
+       <screen format="linespecific">gpsbabel -i gpx -f track.gpx -x interpolate,distance=2m -o gpx -f newtrack.gpx</screen>
+       <para>Reads track.gpx and inserts points wherever two adjacent
+       trackpoints are more than 2 miles apart</para>
+      </section> 
    </chapter>
 </book>
index 9c75289dbb12075bb91ef155de4b96bbd5ea10e1..6238225c9cfdf36fe0c01159bbb2173ecb41be6a 100644 (file)
@@ -102,23 +102,6 @@ track_del_head(route_head *rte)
        trk_head_ct--;
 }
 
-void
-route_debug_count( char *str, queue *q )
-{
-       queue *elem, *tmp;
-       int count = 0;
-       QUEUE_FOR_EACH( q, elem, tmp ) {
-               count++;
-       }
-       fprintf( stderr, "%s %d\n", str, count );
-}
-
-void 
-route_debug_count_main( char *str ) 
-{
-       route_debug_count( str, &my_route_head );
-}
-
 static 
 route_head *
 common_route_by_name(queue *routes, const char *name)
@@ -153,12 +136,14 @@ any_route_add_wpt(route_head *rte, waypoint *wpt, int *ct )
 {
        ENQUEUE_TAIL(&rte->waypoint_list, &wpt->Q);
        rte->rte_waypt_ct++;    /* waypoints in this route */
-       (*ct)++;
-       if (wpt->shortname == NULL) {
-               char tmpnam[10];
-               snprintf(tmpnam, sizeof(tmpnam), "RPT%03d",*ct);
-               wpt->shortname = xstrdup(tmpnam);
-               wpt->wpt_flags.shortname_is_synthetic = 1;
+       if ( ct ) {
+               (*ct)++;
+               if (wpt->shortname == NULL) {
+                       char tmpnam[10];
+                       snprintf(tmpnam, sizeof(tmpnam), "RPT%03d",*ct);
+                       wpt->shortname = xstrdup(tmpnam);
+                       wpt->wpt_flags.shortname_is_synthetic = 1;
+               }
        }
 }
 
@@ -168,6 +153,12 @@ route_add_wpt( route_head *rte, waypoint *wpt )
        any_route_add_wpt( rte, wpt, &rte_waypts );
 }
 
+void 
+track_add_wpt( route_head *rte, waypoint *wpt )
+{
+       any_route_add_wpt( rte, wpt, NULL );
+}
+
 waypoint *
 route_find_waypt_by_name( route_head *rh, const char *name )
 {
index dd25681370292c3fd21fc974d4b5f4173e085f67..6030fa548ccf5244f0befdcae0dcfa42c37d561c 100755 (executable)
@@ -425,7 +425,7 @@ compare ${TMPDIR}/maggpx.trk ${TMPDIR}/gpxtrack.gpx
 rm -f ${TMPDIR}/route.mapsend
 ${PNAME} -r -i mapsend -f reference/route/route.mapsend -o mapsend \
          -F ${TMPDIR}/route.mapsend
-compare ${TMPDIR}/route.mapsend reference/route/
+bincompare ${TMPDIR}/route.mapsend reference/route/route.mapsend
 
 #
 # MAPSEND track format 
@@ -969,6 +969,15 @@ ${PNAME} -i gpx -f ${TMPDIR}/alltypes.gpx -x nuketypes,waypoints,tracks -o gpx -
 ${PNAME} -i gpx -f ${TMPDIR}/wpts.gpx -f ${TMPDIR}/trks.gpx -f ${TMPDIR}/rtes.gpx -o gpx -F ${TMPDIR}/merged.gpx
 compare ${TMPDIR}/alltypes.gpx ${TMPDIR}/merged.gpx
 
+#
+# Interpolate filter
+# 
+
+${PNAME} -i gpx -f reference/track/simpletrack.gpx -x interpolate,distance=50m -o gpx -F ${TMPDIR}/interp.gpx
+compare reference/track/interptrack.gpx ${TMPDIR}/interp.gpx 
+${PNAME} -i gpx -f reference/track/simpletrack.gpx -x interpolate,time=1 -o gpx -F ${TMPDIR}/tinterp.gpx
+compare reference/track/tinterptrack.gpx ${TMPDIR}/tinterp.gpx 
+
 #
 # Universal CSV - unicsv
 #